

import Interface.BluetoothNXT;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;

public class WallFollowerV2 {
	private static Motor motorL = Motor.B;
	private static Motor motorR = Motor.C;
	private static UltrasonicSensor sonicSensor;
	private static TouchSensor touchSensor;

	// degree per second (dps)
	private static int motorSpeedL = 360;
	private static int motorSpeedR = 360;
	private static int defaultSpeed = 360;

	private static int maxSpeed = 500;
	private static int minSpeed = 0;
	private static int bumpAngle = 20;
	private static int unitCorrection = 1; //In CM
	

	private static int reading = 0;

	private static TachoPilot pilot = new TachoPilot(2.20f, 6.30f, motorL,
			motorR);

	public static void main(String[] args) throws Exception {
		sonicSensor = new UltrasonicSensor(SensorPort.S2);
		touchSensor = new TouchSensor(SensorPort.S4);
		
		motorL.setSpeed(motorSpeedL);
		motorR.setSpeed(motorSpeedR);
		
		//bluetooth
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER)
		{		
			BluetoothNXT test = new BluetoothNXT(pilot);
			test.start();
		}
		
		action();
	}

	private static void action() throws InterruptedException {
		
		if(sonicSensor.getUnits().equals("10e-2")){
			unitCorrection = 10; // in mm
		}		
		pilot.forward();
		while (true) {			
			if(touchSensor.isPressed()){	//Tegen de muur gereden			
			correctBump(pilot, sonicSensor, bumpAngle);		
			}
			reading = sonicSensor.getDistance();
			LCD.drawString("reading: " + reading , 0, 0);
			// Motorspeed from maxSpeed dps to minSpeed dps
			motorSpeedR = (int) (maxSpeed + (-maxSpeed + minSpeed) / 25 * unitCorrection * Math.abs(reading));
			if (motorSpeedL > maxSpeed)
				motorSpeedL = maxSpeed;
			if (motorSpeedL < minSpeed + 50)
				motorSpeedL = minSpeed;

			motorSpeedL = (int) (minSpeed + (-minSpeed + maxSpeed) / 25 * unitCorrection * Math.abs(reading));
			if (motorSpeedR > maxSpeed)
				motorSpeedR = maxSpeed;
			if (motorSpeedR < minSpeed + 50)
				motorSpeedR = minSpeed;

			motorL.setSpeed(motorSpeedL);
			motorR.setSpeed(motorSpeedR);

			
			if (reading > 25 * unitCorrection )
			{
				lookForWall();
			}
			
			if (reading < 10  * unitCorrection)
			{
				sharpLeftTurn();
			}
		}
	}

	private static void sharpLeftTurn() {
		System.out.println(reading+ ": Left!");	
		
		pilot.stop();
		motorL.setSpeed(defaultSpeed);
		motorR.setSpeed(defaultSpeed);
		
		pilot.rotate(5);
		
		//Thread.sleep(500);
		pilot.forward();
	}

	private static void sharpRightTurn() {
		System.out.println(reading+ ": Right!");	

		pilot.stop();
		motorL.setSpeed(defaultSpeed);
		motorR.setSpeed(defaultSpeed);
		
		pilot.rotate(-5);	
		
		//Thread.sleep(500);		
		pilot.forward();
	}
	
	private static void lookForWall() {
		
	}
	
	private static void correctBump(TachoPilot pilot, UltrasonicSensor sonicSensor, int bumpAngle) throws InterruptedException {
		LCD.drawString("Bump!", 0, 1); //Thread.sleep(500);
		pilot.travel(-8*unitCorrection);
		int originalDistance = sonicSensor.getDistance();
		pilot.rotate(90);
		if(sonicSensor.getDistance() > originalDistance){
			pilot.rotate(-180);
		}
	}
}
